#include "rkf45.h"
#include "func.h"
#include "zadacha.h"

#include <iostream>

#define max(a,b) (a < b ? b : a)

double rkf45 (double t, double th, std::valarray<double> x, std::valarray<double> w, double* x1, input& data) {
    unsigned m = data.q;
    std::valarray<double> xd(m),k1(m),k2(m),k3(m),k4(m),k5(m),k6(m),dydx(m),x2(m);

    std::valarray<double> u(data.num_upr);
    u = upr(t,w,data);
    dydx = right_parts(x,t,u,data);

    for (unsigned i=0; i<m; i++) k1[i] = th*dydx[i];

    for (unsigned i=0; i<m; i++) xd[i] = x[i] + k1[i]/4;
    u = upr(t+th/4,w,data);
    dydx = right_parts(xd,t+th/4,u,data);
    for (unsigned i=0; i<m; i++) k2[i] = th*dydx[i];


    for (unsigned i=0; i<m; i++) xd[i] = x[i] + (3.0*k1[i] + 9.0*k2[i])/32.0;
    u = upr(t+3*th/8,w,data);
    dydx = right_parts(xd,t+3*th/8,u,data);
    for (unsigned i=0; i<m; i++) k3[i] = th*dydx[i];

    for (unsigned i=0; i<m; i++) xd[i] = x[i] + (1932.0*k1[i] - 7200.0*k2[i] + 7296.0*k3[i])/2197.0;
    u = upr(t+12*th/13,w,data);
    dydx = right_parts(xd,t+12*th/13,u,data);
    for (unsigned i=0; i<m; i++) k4[i] = th*dydx[i];

    for (unsigned i=0; i<m; i++) xd[i] = x[i] + 439.0*k1[i]/216.0 - 8.0*k2[i] + 3680.0*k3[i]/513.0 - 845.0*k4[i]/4104.0;
    u = upr(t+th,w,data);
    dydx = right_parts(xd,t+th,u,data);
    for (unsigned i=0; i<m; i++) k5[i] = th*dydx[i];

    for (unsigned i=0; i<m; i++) xd[i] = x[i] - 8.0*k1[i]/27.0 + 2.0*k2[i] - 3544.0*k3[i]/2565.0 + 1859.0*k4[i]/4104.0 - 11.0*k5[i]/40.0;
    u = upr(t+th/2,w,data);
    dydx = right_parts(xd,t+th/2,u,data);
    for (unsigned i=0; i<m; i++) k6[i] = th*dydx[i];

    double oshibka = 0, max_oshibka = 0,
          otn_pogr = 1e-11,//1e-11,//1e-10,//1e-12,
          abs_pogr = 1e-10,//1e-10,//1e-9,//1e-12,
           pogr = 0;

    for (unsigned i=0; i<m; i++) {
        x1[i] = x[i] + 25*k1[i]/216 + 1408*k3[i]/2565 + 2197*k4[i]/4104 - k5[i]/5;
        x2[i] = x[i] + 16*k1[i]/135 + 6656*k3[i]/12825 + 28561*k4[i]/56430 - 9*k5[i]/50 + 2*k6[i]/55;
        oshibka = fabs(x2[i]-x1[i]);
        pogr = x1[i]*otn_pogr + abs_pogr;
        max_oshibka = max(max_oshibka,oshibka/pogr);
    }
    return max_oshibka;
}
